#ifndef KALMAN_H
#define KALMAN_H
#include "../src/headfile.h"
class KalmanFiter
{
public:
    KalmanFiter(int num_beams);
    void predict();
    void update(const Eigen::VectorXd &measurement);
    Eigen::VectorXd getState() const;

private:
    Eigen::VectorXd state;      // 状态向量
    Eigen::MatrixXd covariance; // 协方差矩阵
    // 其他卡尔曼滤波参数和矩阵
    Eigen::MatrixXd F;
    Eigen::MatrixXd H;
    Eigen::MatrixXd R;
    Eigen::MatrixXd Q;
};
#endif